#ifndef _PARA_H
#define _PARA_H

#include <NL/nl.h>

class LSCM 
{
public:
	typedef float value_type ;

	typedef math::vector<value_type, 2> vector2 ;
	typedef math::vector<value_type, 3> vector3 ;
	typedef math::vector<value_type, 2> point2 ;
	typedef math::vector<value_type, 3> point3 ;

	typedef math::PseudoMesh <value_type, 3> pseudomesh_type ;
	typedef math::HierarchyMesh<math::hierarchy_mesh_traits<value_type, 3> >  mesh_type ;
	typedef math::hierarchy_mesh_traits <value_type, 3>::FaceType face_type ;
	typedef math::hierarchy_mesh_traits <value_type, 3>::VertexType vertex_type ;
	typedef math::hierarchy_mesh_traits <value_type, 3>::HalfEdgeType halfedge_type ;
 

    LSCM(pseudomesh_type & m) : mesh_(&m) ,vxmin(NULL), vxmax(NULL)
	{
		verticesSize = mesh_->idx_pseudo2mesh.size();
    }
	~ LSCM() 
	{
		vxmin->locked = false;
		vxmax->locked = false;
	}
    void apply() {
		//int nb_vertices = mesh_->vertices.size() ;
		int nb_vertices = verticesSize ;
        project() ;
        nlNewContext() ;

        nlSolverParameteri(NL_SOLVER, NL_CG) ;
        nlSolverParameteri(NL_PRECONDITIONER, NL_PRECOND_JACOBI) ;
        nlSolverParameteri(NL_NB_VARIABLES, 2*nb_vertices) ;
        nlSolverParameteri(NL_LEAST_SQUARES, NL_TRUE) ;
        nlSolverParameteri(NL_MAX_ITERATIONS, 5*nb_vertices) ;
        nlSolverParameterd(NL_THRESHOLD, 1e-10) ;
        nlBegin(NL_SYSTEM) ;
        mesh_to_solver() ;
        nlBegin(NL_MATRIX) ;
        setup_lscm() ;
        nlEnd(NL_MATRIX) ;
        nlEnd(NL_SYSTEM) ;
//        std::cout << "Solving ..." << std::endl ;
        nlSolve() ;
        solver_to_mesh() ;
        double time ;
        NLint iterations;
        nlGetDoublev(NL_ELAPSED_TIME, &time) ;
        nlGetIntergerv(NL_USED_ITERATIONS, &iterations); 
//      std::cout << "Solver time: " << time << std::endl ;
//      std::cout << "Used iterations: " << iterations << std::endl ;
        nlDeleteContext(nlGetCurrent()) ;
    }

protected:

    void setup_lscm() {
//		std::cout<<"FACE SIZE "<<mesh_->faces.size()<<std::endl;
		for(unsigned int i=0; i<mesh_->faces.size() ; i++) {
            setup_lscm(mesh_->faces[i]) ;
        }
    }

    // Note: no-need to triangulate the facet,
    // we can do that "virtually", by creating triangles
    // radiating around vertex 0 of the facet.
    // (however, this may be invalid for concave facets)
    void setup_lscm(const face_type  *  face) {  //FIXME HERE
                
		halfedge_type * iter = face->he;

		setup_conformal_map_relations( * mesh_->getMesh()->vertices[iter->from_v->m_idx] ,
									   * mesh_->getMesh()->vertices[iter->next->from_v->m_idx], 
									   * mesh_->getMesh()->vertices[iter->next->next->from_v->m_idx]) ;

    }

    // Computes the coordinates of the vertices of a triangle
    // in a local 2D orthonormal basis of the triangle's plane.
    static void project_triangle(
        const vector3& p0, 
        const vector3& p1, 
        const vector3& p2,
        vector2& z0,
        vector2& z1,
        vector2& z2) 
	{
        vector3 X = p1 - p0 ;
        X.normalize() ;
        vector3 Z = X ^ (p2 - p0) ;
        Z.normalize() ;
        vector3 Y = Z ^ X ;
        const vector3& O = p0 ;
        
        float x0 = 0 ;
        float y0 = 0 ;
        float x1 = distance (p1, O) ;//(p1 - O).length() ;
        float y1 = 0 ;
        float x2 = (p2 - O) * X ;
        float y2 = (p2 - O) * Y ;        
            
        z0 = vector2(x0,y0) ;
        z1 = vector2(x1,y1) ;
        z2 = vector2(x2,y2) ;        
    }

    // LSCM equation, geometric form :
    // (Z1 - Z0)(U2 - U0) = (Z2 - Z0)(U1 - U0)
    // Where Uk = uk + i.vk is the complex number 
    //                       corresponding to (u,v) coords
    //       Zk = xk + i.yk is the complex number 
    //                       corresponding to local (x,y) coords
    // cool: no divide with this expression,
    //  makes it more numerically stable in
    //  the presence of degenerate triangles.
    
    void setup_conformal_map_relations(
		vertex_type & v0,  vertex_type & v1,  vertex_type & v2 )
    {
        
		int id0 = mesh_->mesh2pseudo_idx(v0.m_idx) ;
        int id1 = mesh_->mesh2pseudo_idx(v1.m_idx) ;
        int id2 = mesh_->mesh2pseudo_idx(v2.m_idx) ;
            
        const vector3& p0 = v0.m_coord ; //point ; 
        const vector3& p1 = v1.m_coord ; //point ;
        const vector3& p2 = v2.m_coord ; //point ;
            
        vector2 z0,z1,z2 ;
        project_triangle(p0,p1,p2,z0,z1,z2) ;
        vector2 z01 = z1 - z0 ;
        vector2 z02 = z2 - z0 ;
        double a = z01.x ;
        double b = z01.y ;
        double c = z02.x ;
        double d = z02.y ;
        assert(b == 0.0) ;

        // Note  : 2*id + 0 --> u
        //         2*id + 1 --> v
        int u0_id = 2*id0     ;
        int v0_id = 2*id0 + 1 ;
        int u1_id = 2*id1     ;
        int v1_id = 2*id1 + 1 ;
        int u2_id = 2*id2     ;
        int v2_id = 2*id2 + 1 ;
        
        // Note : b = 0

        // Real part
        nlBegin(NL_ROW) ;
        nlCoefficient(u0_id, -a+c)  ;
        nlCoefficient(v0_id,  b-d)  ;
        nlCoefficient(u1_id,   -c)  ;
        nlCoefficient(v1_id,    d)  ;
        nlCoefficient(u2_id,    a) ;
        nlEnd(NL_ROW) ;

        // Imaginary part
        nlBegin(NL_ROW) ;
        nlCoefficient(u0_id, -b+d) ;
        nlCoefficient(v0_id, -a+c) ;
        nlCoefficient(u1_id,   -d) ;
        nlCoefficient(v1_id,   -c) ;
        nlCoefficient(v2_id,    a) ;
        nlEnd(NL_ROW) ;
    }


	// copies u,v coordinates from OpenNL solver to the mesh.
    void solver_to_mesh() {
		for(unsigned int i=0; i< this->verticesSize; i++) {
			vertex_type * it = mesh_->getMesh()->vertices[mesh_->pseudo2mesh_idx(i)]; 
			float u = nlGetVariable(2 * i    ) ;
            float v = nlGetVariable(2 * i + 1) ;
            it->m_texcoord = vector2(u,v) ;
        }
    }
        
    //copies u,v coordinates from the mesh to OpenNL solver.
    void mesh_to_solver() {
		for(unsigned int i=0; i<this->verticesSize; i++) {
            vertex_type * it = mesh_->getMesh()->vertices[mesh_->pseudo2mesh_idx(i)] ;
            float u = it->m_texcoord.x ;
            float v = it->m_texcoord.y ;
			//std::cout<<u<<' '<<v<<std::endl;
            nlSetVariable(2 * i   , u) ;
            nlSetVariable(2 * i + 1, v) ;
            if(it->locked) {
                nlLockVariable(2 * i    ) ;
                nlLockVariable(2 * i + 1) ;
            } 
        }
    }

    // Chooses an initial solution, and locks two vertices
    void project() {
        // Get bbox
        unsigned int i ;

        float xmin =  1e5 ;
        float ymin =  1e5 ;
        float zmin =  1e5 ;
        float xmax = -1e5 ;
        float ymax = -1e5 ;
        float zmax = -1e5 ;

        for(i=0; i< verticesSize; ++i) {
            const vertex_type *  v = mesh_->getMesh()->vertices[mesh_->pseudo2mesh_idx(i)] ;
            xmin = std::min(v->m_coord.x, xmin) ;
            ymin = std::min(v->m_coord.y, xmin) ;
            zmin = std::min(v->m_coord.z, xmin) ;

            xmax = std::max(v->m_coord.x, xmin) ;
            ymax = std::max(v->m_coord.y, xmin) ;
            zmax = std::max(v->m_coord.z, xmin) ;
        }

        float dx = xmax - xmin ;
        float dy = ymax - ymin ;
        float dz = zmax - zmin ;
        
//		std::cout<<"DX "<<dx<<std::endl;
//		std::cout<<"DY "<<dy<<std::endl;
//		std::cout<<"DZ "<<dz<<std::endl;
        vector3 V1,V2 ;

        // Find shortest bbox axis
        if(dx < dy && dx < dz) {
            if(dy > dz) {
                V1 = vector3(0,1,0) ;
                V2 = vector3(0,0,1) ;
            } else {
                V2 = vector3(0,1,0) ;
                V1 = vector3(0,0,1) ;
            }
        } else if(dy < dx && dy < dz) {
            if(dx > dz) {
                V1 = vector3(1,0,0) ;
                V2 = vector3(0,0,1) ;
            } else {
                V2 = vector3(1,0,0) ;
                V1 = vector3(0,0,1) ;
            }
        } else  {
            if(dx > dy) {
                V1 = vector3(1,0,0) ;
                V2 = vector3(0,1,0) ;
            } else {
                V2 = vector3(1,0,0) ;
                V1 = vector3(0,1,0) ;
            }
        }

        // Project onto shortest bbox axis,
        // and lock extrema vertices

        //Vertex* vxmin = NULL ;
        float  umin = 1e5 ;
        
        float  umax = -1e5 ;

        for(i=0; i<verticesSize; i++) {
            vertex_type * V = mesh_->getMesh()->vertices[mesh_->pseudo2mesh_idx(i)] ;
            float u = V->m_coord * V1 ;
            float v = V->m_coord * V2 ;
            V->m_texcoord = vector2(u,v) ;
            if(u < umin) {
                vxmin = V ;
                umin = u ;
            } 
            if(u > umax) {
                vxmax = V ;
                umax = u ;
            } 
        }

        vxmin->locked = true ;
        vxmax->locked = true ;

		//This fucntion is correct
		//std::cout<<"VMIN "<<vxmin->idx<<std::endl;
		//std::cout<<"VMAX "<<vxmax->idx<<std::endl;
    }

    pseudomesh_type * mesh_ ;
private:
	vertex_type * vxmin ;
	vertex_type * vxmax ;
	unsigned verticesSize;
} ;


class OPAffineTransformation 
{
protected :
	typedef float value_type ;

	typedef math::vector<value_type, 2> vector2 ;
	typedef math::vector<value_type, 3> vector3 ;
	typedef math::vector<value_type, 2> point2 ;
	typedef math::vector<value_type, 3> point3 ;

	typedef math::PseudoMesh <value_type, 3> pseudomesh_type ;
	typedef math::HierarchyMesh<math::hierarchy_mesh_traits<value_type, 3> >  mesh_type ;
	typedef math::hierarchy_mesh_traits <value_type, 3>::FaceType face_type ;
	typedef math::hierarchy_mesh_traits <value_type, 3>::VertexType vertex_type ;
	typedef math::hierarchy_mesh_traits <value_type, 3>::HalfEdgeType halfedge_type ;
public :
        OPAffineTransformation (pseudomesh_type * pmesh, vector2 & trans, float scale, float _rotate_cos, float _rotate_sin): 
           trans(trans), scale(scale), rotate_cos (_rotate_cos), rotate_sin (_rotate_sin)
        { this->pMesh = pmesh;}
        virtual operator pseudomesh_type * ()
        {
                std::set<vertex_type * > vertexSet ;
                for (unsigned i = 0;i < pMesh->faces.size(); ++i)
                {
					std::vector<int> idx;
					pMesh->faces[i]->Face2VertexIdx(idx);
					vertexSet.insert(pMesh->getMesh()->vertices[idx[0]]);
					vertexSet.insert(pMesh->getMesh()->vertices[idx[1]]);
					vertexSet.insert(pMesh->getMesh()->vertices[idx[2]]);
                }
                //Do translate -> scaling -> translate back ;
                for (std::set<vertex_type * >::iterator iter = vertexSet.begin(); iter != vertexSet.end(); ++iter)
                {
                        (* iter)->m_texcoord = (* iter)->m_texcoord - trans ; // trans
						
                        (* iter)->m_texcoord = (* iter)->m_texcoord * scale ; // scaling;
                        
                        float x =  (* iter)->m_texcoord.x ;   // Rotation 
                        float y =  (* iter)->m_texcoord.y;
                        (* iter)->m_texcoord.x = x * rotate_cos - y * rotate_sin ; 
                        (* iter)->m_texcoord.y = x * rotate_sin + y * rotate_cos ;
                        
        
                }
                return pMesh; 
        }


		static point2 compute_translation (face_type * center_face, Brush * brush)
		{
			std::vector<point3 *> v_o ;
			point3 v_w[3];
			center_face->Face2VertexCoord(v_o);
			v_w[0] = brush->mappingObj2Win(*v_o[0]);
			v_w[1] = brush->mappingObj2Win(*v_o[1]);
			v_w[2] = brush->mappingObj2Win(*v_o[2]);
			point3 v_b (brush->get_win_pos().x, brush->get_win_pos().y, 1);

			math::matrix<value_type, 3, 3> matrix, invmatrix; 
			matrix[0][0] = v_w[0].x ; matrix[0][1] = v_w[1].x ; matrix[0][2] = v_w[2].x ;
			matrix[1][0] = v_w[0].y ; matrix[1][1] = v_w[1].y ; matrix[1][2] = v_w[2].y ;
			matrix[2][0] = 1.0f             ; matrix[2][1] = 1.0f     ; matrix[2][2] = 1.0f ; 

		
			invmatrix = matrix.inv();
			vector3 barycoord = invmatrix * v_b ;

			//Affine transformation
			std::vector<point2 *> v;
			center_face->Face2TexCoord(v);
			point2 center = (*v[0]) * barycoord[0] + (*v[1]) * barycoord[1] + (*v[2]) * barycoord[2] ;
			return center ;
		}
        //This is an auxiliary function which helps to determine the affine transformation 
		static bool compute_affine (face_type * face, Brush * brush,
									point2 & trans, float & scale, float & rotate_cos, float & rotate_sin)
		{
			
			if (face == NULL) return false; 

			std::vector<point3 *> coord; 
			face->Face2VertexCoord(coord);
			std::vector<point2 *> texcoord; 
			face->Face2TexCoord(texcoord);
			trans = compute_translation(face, brush);

			point3  p[3];
			p[0] = brush->objVM.mappingObj2Win(*coord[0]) ;
			p[1] = brush->objVM.mappingObj2Win(*coord[1]) ;
			p[2] = brush->objVM.mappingObj2Win(*coord[2]) ;

			value_type length1 = distance(p[0] , p[1]);
			value_type length2 = distance(p[1] , p[2]);
			value_type length3 = distance(p[2] , p[0]);

			int i ,j ;
			if (length1 > length2 && length1 > length3)
			{
                i = 0; j = 1; 
			}
			else if (length2 > length1 && length2 > length3)
			{
                i = 1 ;j = 2;
			}
			else
			{
                i = 2 ; j = 0;
			}

			point3 win_p1 = p[i] ;
			point3 win_p2 = p[j];

			point3 tex_p1 = brush->texVM.mappingObj2Win(*texcoord[i]);
			point3 tex_p2 = brush->texVM.mappingObj2Win(*texcoord[j]);

			//Compute scaling ;
			//float win_length =  length1 ;
			float tex_zbuffer = brush->texVM.mappingObj2Win(*texcoord[i]).z ;
			win_p1.z = win_p2.z = tex_zbuffer ;
			float tex_length = distance (brush->texVM.mappingWin2Obj(win_p1) , brush->texVM.mappingWin2Obj(win_p2));
			scale = tex_length / (*texcoord[i] - *texcoord[j]).module();


			//Compute rotate ;
			vector3 v_obj = win_p1 - win_p2 ;
			vector3 v_tex = tex_p1 - tex_p2 ;
			v_obj.z = v_tex.z = 0;
			//compute cos(\theta)
			float cos_theta = v_obj * v_tex /(v_obj.module() * v_tex.module()) ;
			rotate_cos = cos_theta ;
			//compute sin(\theta)
			vector3 cross_product = v_tex ^ v_obj  ;
			float sin_theta =  cross_product.module () / (v_obj.module() * v_tex.module()) ;
			if (cross_product.z > 0 ) sin_theta = - sin_theta ;
			rotate_sin = sin_theta ;
			
			return true; 
		}


private:
        pseudomesh_type * pMesh ; 
        vector2 trans ;
        float scale ;
        float rotate_cos ;
        float rotate_sin ;
} ;

#endif
